/* VKOS_Thor
     * Lighting controller for 8pc acoustic drum kit, using piezos to trigger.
     [Configuration is modified via serial from host control system (RPi)]
*/

#include <string.h>
#include "Serial_Thor.h"

// ShiftPWM ----------------------------------------------
const int ShiftPWM_latchPin=8;      // ST_CP
#define SHIFTPWM_NOSPI
const int ShiftPWM_dataPin = 11;    // DS
const int ShiftPWM_clockPin = 12;   // SH_CP
const bool ShiftPWM_invertOutputs = false; 
const bool ShiftPWM_balanceLoad = false;
#include <ShiftPWM.h> // Needs to be declared after pins
unsigned char maxBrightness = 255;
unsigned char pwmFrequency = 75;
int numRegisters = 3;
const unsigned int numRGBleds = (numRegisters*8/3);
//--------------------------------------------------------
unsigned long lTime;        // Running MS count since boot


/*#########################
  ## VKC_COLOR constants ##
  #########################*/
const unsigned int VKC_BLUE = 2;
const unsigned int VKC_GREEN = 3;



/*###########################
  ## Function Declarations ##
  ###########################*/
void initComms();
void HandleCommand(String strCmd);



/*###########################
  ## Drum object consts ##
  ########################*/
const unsigned int DS_OFF = 39;
const unsigned int DS_FADE_IN = 40; const unsigned int DS_FADE_OUT = 41;



// objDrumTrigger
typedef struct {
  public:
    // General vars
    unsigned int iID;            // Index ID
    unsigned int iA;             // Real-Time piezo level (analog)
    unsigned int iColor;         // VKC_xxx color index
    unsigned int iFS;            // Fade step
    unsigned int iState;         // Current light state
    unsigned int iThresh;        // Trigger threshold
    unsigned long lLast;         // Last time (MS) drum was triggered
    
    // Attempt to minimize rapid re-triggering
    bool canTrigger()          {return (bool)((lTime-this->lLastTrig)>=100);}
    
    // Management functions
    void init(unsigned int iID); // Initialize light object
    void ckDrum();   // Process analog input
    void update();   // Update light brightness/programming
} objDrumTrigger;



/*####################
  ## Global objects ##
  ####################*/
const String ID_STR = "[ID]THOR";
static objDrumTrigger oDrums[8];       // Instance drum objects
static String strTrigVals;
static String strInp = "";
static boolean bSerial = false;        // Serial connection status
static boolean bHandshook = false;     // ID/sync indication
static boolean bCmdReady = false;      // Command has been received
const unsigned long COMMS_BAUD_RATE = 115200;
const String PACKETHEADER = "!!!";
const unsigned int CMD_OK = 999;
const unsigned int CMD_NONE = 0;



/*----------------------------------------*
 | Func: objDrumTrigger::init()           |
 | Desc: Set up drum trigger object.      |
 *----------------------------------------*/
void objDrumTrigger::init(unsigned int iID) {
  switch(iID) {
    case 0:    // Kick #1
      this->iThresh = 100;
      this->iColor = VKC_BLUE;
      break;
    
    case 1:    // Kick #2
      this->iThresh = 100;
      this->iColor = VKC_BLUE;
      break;
    
    case 2:    // Snare
      this->iThresh = 200;
      this->iColor = VKC_GREEN;
      break;
    
    case 3: this->iThresh = 160; break;   // Tom #1
    case 4: this->iThresh = 160; break;   // Tom #2
    case 5: this->iThresh = 160; break;   // Tom #3
    case 6: this->iThresh = 160; break;   // Tom #4
    case 7: this->iThresh = 200; break;   // Floor Tom
  }
  
  this->iColor = VKC_BLUE;   // Default color is blue
  this->iState = DS_OFF;     // Default state = OFF
  this->iID = iID;           // Set object's index
}



/*----------------------------------------*
 | Func: objDrumTrigger::ckDrum()         |
 | Desc: Gets trigger level and determine |
 | if light needs to be lit/re-lit.       |
 *----------------------------------------*/
void objDrumTrigger::ckDrum() {
  // Read & record
  this->iA = analogRead(this->iID);  // Get current analog value
  
  // React
  if(this->iA >= this->
  
  //Serial.print("iA = "); Serial.print(iA,DEC); Serial.println("");
  //if(!this->canTrigger()) {return;}  // Exit here if light cannot be triggered
  if((this->iA >= this->iThresh) && (this->iState == DS_OFF)) {  // Check if we need to trigger...
    this->iState = DS_FADE_IN;  // Reset fade in timer (or begin, if off)
    this->lLastTrig = millis();
    Serial.print("Port #"); Serial.print(this->iID,DEC); Serial.print(" --- iA="); Serial.print(iA,DEC); Serial.println("");
  }
}


/*----------------------------------------*
 | Func: objDrumTrigger::updateLight()    |
 | Desc: Update lighting timers/levels.   |
 *----------------------------------------*/
void objDrumTrigger::updateLight() {
  lTime = millis();  // Update master clock
  
  if(this->iState != DS_OFF) {
    // Update brightness
    ShiftPWM.SetHSV(this->iID,60,320,this->iFadeStep+64);
    
    // Adjust as needed
    //if(this->iState == DS_FADE_IN) {this->iFadeStep += 15;}
    //if(this->iState == DS_FADE_OUT) {this->iFadeStep -= 15;}
    
    switch(iState) {
      case DS_FADE_IN:
        iFadeStep += 15;
        if(iFadeStep>=255) {
          Serial.print("Light #"); Serial.print(this->iID,DEC); Serial.println(" ON");
          iState = DS_FADE_OUT; iFadeStep = 255; ShiftPWM.SetHSV(this->iID,60,320,320); delay(250);
        }
        break;
      case DS_FADE_OUT:
        iFadeStep -= 15;
        if(iFadeStep<=0) {
          Serial.print("Light #"); Serial.print(this->iID,DEC); Serial.println(" OFF");
          iState = DS_OFF; ShiftPWM.SetRGB(this->iID,0,0,0,0);
        }
        break;
    }
    
    // Sanity check
    if(this->iState > 255) {this->iState = 255;}  // Make sure we don't go past max brightness
  }
}



/*----------------------------------------*
 | [Arduino - Boot]                       |
 *----------------------------------------*/
void setup() {
  Serial.begin(COMMS_BAUD_RATE);  // Python access from RPi
  ShiftPWM.SetAmountOfRegisters(numRegisters);
  ShiftPWM.Start(pwmFrequency,maxBrightness);
  ShiftPWM.SetPinGrouping(1);
  ShiftPWM.SetAll(0);
  
  // Setup drum trigger objects
  for(unsigned int d=0; d<numRGBleds; d++) {oDrums[d].init(d);}
}


/*-----------------------------------------*
 | [Arduino - Main loop]                   |
 *-----------------------------------------*/
void loop() {
  //getMessage(); // Check for serial activity
  
  //strTrigVals = "[" + String(millis()) + "] Triggers: ";
  // Check all triggers
  for(unsigned int d=0; d<numRGBleds; d++) {
    //Serial.print("oDrums["); Serial.print(d,DEC); Serial.println("]");
    oDrums[d].ckDrum();      // Check analog input and react accordingly
    oDrums[d].updateLight(); // Update light and timers, if needed
    //strTrigVals += "p" + String(d) + ": " + String(oDrums[d].iA) + " -";
  }
  //Serial.println(strTrigVals);
  delay(10);
}



/*-----------------------------------------------------*
 | Func: initComms():                                  |
 | Desc: Initiate/assert serial link to control center |
 *-----------------------------------------------------*/
void initComms() {
  boolean bResponse = false; bHandshook = false;
  String sHSBuff = ""; char inHS = ' ';
  
  // Establish link... Loop and block until successful.
  while(!bSerial) { 
    Serial.begin(COMMS_BAUD_RATE);
    delay(500); bSerial = (Serial);
    //if(!bSerial) {flashLED(LED_SOS,GENERAL_DELAY,VKC_RED);}
  }
  
  // Need to ID to main system
  while(!bResponse) {  // Block for response
    //setAllLamps(0,LS_ON,VKC_YELLOW); 
    Serial.println(ID_STR); Serial.flush();
    
    // Wait and check for response (~10sec)
    for(int iW=0; iW<100; iW++) {
      delay(100);
      if(Serial.available()) {
        iW=100; bResponse=true;
      }
    }
    
    // Determine if we need to try again
    //if(!bResponse) {flashLED(LED_SOS, GENERAL_DELAY, VKC_YELLOW);}
  }
  
  // Wait for and process handshake response
  //setAllLamps(0, LS_ON, VKC_ORANGE);
  while(!bHandshook) {
    if(Serial.available()) {
      inHS = (char)Serial.read();
      if(inHS=='\n') {
        //Serial.println("Got: "+sHSBuff);
        if(sHSBuff.toInt() == CMD_OK) {bHandshook=true;}
        else {sHSBuff="";}  // Didn't get CMD_OK; Clear buffer and try again
      }
      else {sHSBuff+=inHS;} // Add to buffer, wait for '\n'
    }
  }
  
  // Ready to go!
  Serial.println(CMD_OK);
  //flashLED(LED_OK,0,VKC_GREEN);
  strInp = "";  // Weird bug... Ensure this is empty.
  Serial.flush(); delay(5);  // Prep for commands
}

// [ARDUINO] Called on incoming data
void serialEvent() {
  if (Serial.available()) {
    // If we got '\n', process accordingly
    char inC = (char)Serial.read();
    if(inC=='\n') {
      //Serial.println("serialEvent(): strInp="+strInp);
      HandleCommand(strInp+'\n');
      strInp = "";  // Clear buffer
    }
    else {strInp+=inC;}  // Add to cmd buffer
  }
}

void HandleCommand(String strCmd) {
  unsigned int iArg=0; String strTmp=""; boolean bValidCmd=false;
  unsigned int iLight=CMD_NONE; unsigned int iCmd=CMD_NONE; unsigned int iParam=CMD_NONE;
  char cTmp;
  
  // Parse string
  for(unsigned int i=0;i<=strCmd.length();i++) {
    // Process
    cTmp = ' '; cTmp = strCmd.charAt(i);    
    if(cTmp == '^') {
      if(iArg==0) {if(strTmp != PACKETHEADER) {return;}}  // Reject without header
      if(iArg==1) {iCmd = strTmp.toInt();}     // Command index
      if(iArg==2) {iLight = strTmp.toInt();}   // Target light index
      iArg++; strTmp="";
    }
    else if(cTmp == '\n') {  // End of command string
      //if(iCmd == CMD_MASS_CFG) {setMassCfg(iLight, strTmp); return;}
      iParam = strTmp.toInt(); // Avoid need for delimeter after last arg
      bValidCmd = true;
    }
    else {strTmp += cTmp;}  // Copy into buffer
  }
}
